/*
Copyright (C) 2021-2025 Casa Xu (also Zhiyan Xu) from HIT

This program is free software; you can redistribute it and/or
modify it under the terms of the GNU General Public License
as published by the Free Software Foundation; either version 2
of the License, or (at your option) any later version.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program; if not, write to the Free Software
Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
*/

/*
 * LOS_CAN.c
 *
 *  Created on: 2022年1月11日
 *      Author: Casa XU
 */

#include "LOS_CAN.h"
#include "LOS_DataTran.h"
#include "LOS_GNSS.h"
#include "LOS_FATFS.h"
#include "LOS_StarSensor.h"
#include "LOS_FLASH.h"
#include "LOS_FATFS.h"
#include "LOS_State.h"
#include "os_semphr.h"
uint16 rx_data5[8] = {0};


uint8_t flag=0;
extern uint8_t sd_flag;
extern uint8_t max_flag;
extern uint8_t FLASH_flag;
uint8_t fatfs_arg = 0;
uint8_t flash_sta = 0;

LOS_CANMSG_t CAN_Frame_Data[20];

extern LOS_SemaphoreHandle_t BinSemphr_TC;
extern LOS_SemaphoreHandle_t BinSemphr_UP;
/** @fn void LOS_CAN_Init(void)
*   @brief CAN总线初始化函数
*/
void LOS_CAN_Init(void)
{
    canInit();
}


/** @fn uint32_t LOS_CAN_Transmit(uint8_t CAN_NUM, uint32_t ID, uint8_t* data)
*   @brief CAN总线发送函数
*   @param[in] CAN_NUM CAN端口编号:
*              - LOS_CAN_NUM1: CAN1 
*              - LOS_CAN_NUM2: CAN2 
*   @param[in] ID CAN数据仲裁域（29位，补0扩充到32位）
*   @param[in] data 指向待发送数据的指针
*   @return 该函数的返回值为:
*           - 0: 发送失败   
*           - 1: 发送成功
*/
uint32_t LOS_CAN_Transmit(uint8_t CAN_NUM, uint32_t ID, uint8_t* data, size_t datalen)
{
    uint8_t canMESSAGE_BOX = canMESSAGE_BOX1;
    uint32_t tran_check=0x100;
    uint16_t err_cnt = 0;
    static uint8_t turn = 0;
    static uint8_t sta_ma = 0;

    switch (datalen)
    {
        case 1U:
        canMESSAGE_BOX = canMESSAGE_BOX51;
        break;

        case 2U:
        canMESSAGE_BOX = canMESSAGE_BOX52;
        break;

        case 3U:
        canMESSAGE_BOX = canMESSAGE_BOX53;
        break;

        case 4U:
        canMESSAGE_BOX = canMESSAGE_BOX54;
        break;

        case 5U:
        canMESSAGE_BOX = canMESSAGE_BOX55;
        break;

        case 6U:
        canMESSAGE_BOX = canMESSAGE_BOX56;
        break;

        case 7U:
        canMESSAGE_BOX = canMESSAGE_BOX57;
        break;

        case 8U:
        canMESSAGE_BOX = canMESSAGE_BOX1;
        break;

        default:
        canMESSAGE_BOX = canMESSAGE_BOX1;
    }


    ID |= 0X03 << 29;//不能没有这行, trust me

    if (CAN_NUM == LOS_CAN_NUM1)
    {
        while(canIsTxMessagePending(canREG1,canMESSAGE_BOX))//检查上一次发送是否完成
        {
            err_cnt++;
            if (err_cnt >= 5000)
            {
                break;
            }
        }
        canUpdateID(canREG1, canMESSAGE_BOX, ID);
        return canTransmit(canREG1, canMESSAGE_BOX, data);
    }
    else if (CAN_NUM == LOS_CAN_NUM2)
    {
        while(canIsTxMessagePending(canREG2,canMESSAGE_BOX))//检查上一次发送是否完成
        {
            err_cnt++;
            if (err_cnt >= 5000)
            {
                break;
            }
        }
        canUpdateID(canREG2, canMESSAGE_BOX, ID);
        return canTransmit(canREG2, canMESSAGE_BOX, data);
    }
}


/** @fn uint32_t LOS_CAN_Receive(uint8_t CAN_NUM, LOS_CANMSG_t* canmsg)
*   @brief CAN总线接收函数
*   @param[in] CAN_NUM CAN端口编号:
*              - LOS_CAN_NUM1: CAN1 
*              - LOS_CAN_NUM2: CAN2 
*   @param[in] canmsg 存放接收数据的结构体指针，包括帧ID和数据（最大8字节）
*   @return 该函数的返回值为:
*           - 0: 无新数据
*           - 1: 新数据已接收
*           - 3: 新数据已接收，但之前有数据丢失
*/
uint32_t LOS_CAN_Receive(uint8_t CAN_NUM, LOS_CANMSG_t* canmsg, uint32_t canMESSAGE_BOX)
{
    if (CAN_NUM == LOS_CAN_NUM1)
    {
        canmsg->ID = canGetID(canREG1,canMESSAGE_BOX);
        return canGetData(canREG1,canMESSAGE_BOX,canmsg->msg);
    }
    else if (CAN_NUM == LOS_CAN_NUM2)
    {
        canmsg->ID = canGetID(canREG2,canMESSAGE_BOX);
        return canGetData(canREG2,canMESSAGE_BOX,canmsg->msg);
    }
}


/** @fn void canMessageNotification(canBASE_t *node, uint32 messageBox)  
*   @brief CAN接收中断服务函数
*   @param[in] node CAN端口号:
*              - canREG1: CAN1 
*              - canREG2: CAN2 
*   @param[in] messageBox 

*/
void canMessageNotification(canBASE_t *node, uint32 messageBox)
{
    LOS_CANMSG_t CAN_Frame_Re;
    LOS_CANMSG_t CAN_Frame_Permit;
    uint32_t id = 0;
    uint8_t id_filter[4];

    if(node==canREG1)
    {
        if(messageBox==canMESSAGE_BOX2) //测控
        {
            LOS_CAN_Receive(LOS_CAN_NUM1,&CAN_Frame_Re, messageBox);
            id  = CAN_Frame_Re.ID;
            memcpy(id_filter,&id,4);
            if (id_filter[2]==nID_TTCBoard)
            {
                LOS_CAN_TMTCCallBack(&CAN_Frame_Re);
            }
        }
        else if(messageBox==canMESSAGE_BOX3) //数传
        {
            LOS_CAN_Receive(LOS_CAN_NUM1,&CAN_Frame_Re, messageBox);
            id  = CAN_Frame_Re.ID;
            memcpy(id_filter,&id,4);
            if (id_filter[2]==nID_BaseBand)
            {
                LOS_CAN_BaseBandCallBack(&CAN_Frame_Re);
            }
        }
        else if(messageBox==canMESSAGE_BOX4) //存储板
        {
            LOS_CAN_Receive(LOS_CAN_NUM1,&CAN_Frame_Re, messageBox);
            id  = CAN_Frame_Re.ID;
            memcpy(id_filter,&id,4);
            if (id_filter[2]==nID_Storage)
            {
                LOS_CAN_SATACallBack(&CAN_Frame_Re);
            }
        }
        else if(messageBox==canMESSAGE_BOX6) //飞轮X
        {
            LOS_CAN_Receive(LOS_CAN_NUM1,&CAN_Frame_Re, messageBox);
            id  = CAN_Frame_Re.ID;
            memcpy(id_filter,&id,4);
            if (id_filter[2]==nID_WheelX)
            {
                LOS_CAN_FWXCallBack(&CAN_Frame_Re);
            }
        }
        else if(messageBox==canMESSAGE_BOX7) //飞轮Y
        {
            LOS_CAN_Receive(LOS_CAN_NUM1,&CAN_Frame_Re, messageBox);
            id  = CAN_Frame_Re.ID;
            memcpy(id_filter,&id,4);
            if (id_filter[2]==nID_WheelY)
            {
                LOS_CAN_FWYCallBack(&CAN_Frame_Re);
            }
        }
        else if(messageBox==canMESSAGE_BOX8) //飞轮Z
        {
            LOS_CAN_Receive(LOS_CAN_NUM1,&CAN_Frame_Re, messageBox);
            id  = CAN_Frame_Re.ID;
            memcpy(id_filter,&id,4);
            if (id_filter[2]==nID_WheelZ)
            {
                LOS_CAN_FWZCallBack(&CAN_Frame_Re);
            }
        }
        else if(messageBox==canMESSAGE_BOX9) //飞轮S
        {
            LOS_CAN_Receive(LOS_CAN_NUM1,&CAN_Frame_Re, messageBox);
            id  = CAN_Frame_Re.ID;
            memcpy(id_filter,&id,4);
            if (id_filter[2]==nID_WheelS)
            {
                LOS_CAN_FWSCallBack(&CAN_Frame_Re);
            }
        }
        else if(messageBox==canMESSAGE_BOX10) //星敏Y
        {
            LOS_CAN_Receive(LOS_CAN_NUM1,&CAN_Frame_Re, messageBox);
            id  = CAN_Frame_Re.ID;
            memcpy(id_filter,&id,4);
            if (id_filter[2]==nID_StarSensorY)
            {
                LOS_CAN_SSYCallBack(&CAN_Frame_Re);
            }
        }
        else if(messageBox==canMESSAGE_BOX11) //星敏Z
        {
            LOS_CAN_Receive(LOS_CAN_NUM1,&CAN_Frame_Re, messageBox);
            id  = CAN_Frame_Re.ID;
            memcpy(id_filter,&id,4);
            if (id_filter[2]==nID_StarSensorZ)
            {
                LOS_CAN_SSZCallBack(&CAN_Frame_Re);
            }
        }
        else if(messageBox==canMESSAGE_BOX12) //太敏
        {
            LOS_CAN_Receive(LOS_CAN_NUM1,&CAN_Frame_Re, messageBox);
            id  = CAN_Frame_Re.ID;
            memcpy(id_filter,&id,4);
            if (id_filter[2]==nID_SunSensor)
            {
                LOS_CAN_SunSensorCallBack(&CAN_Frame_Re);
            }
        }
        else if(messageBox==canMESSAGE_BOX13) //GNSS
        {
            LOS_CAN_Receive(LOS_CAN_NUM1,&CAN_Frame_Re, messageBox);
            id  = CAN_Frame_Re.ID;
            memcpy(id_filter,&id,4);
            if (id_filter[2]==nID_GNSS)
            {
                LOS_CAN_GNSSCallBack(&CAN_Frame_Re);
            }
        }
        else if(messageBox==canMESSAGE_BOX14) //导航增强
        {
            LOS_CAN_Receive(LOS_CAN_NUM1,&CAN_Frame_Re, messageBox);
            id  = CAN_Frame_Re.ID;
            memcpy(id_filter,&id,4);
            if (id_filter[2]==nID_NaviEnhance)
            {
                LOS_CAN_NaviEnhanceCallBack(&CAN_Frame_Re);
            }
        }
        else if(messageBox==canMESSAGE_BOX15) //时空表
        {
            LOS_CAN_Receive(LOS_CAN_NUM1,&CAN_Frame_Re, messageBox);
            id  = CAN_Frame_Re.ID;
            memcpy(id_filter,&id,4);
            if (id_filter[2]==nID_Spacetime)
            {
                LOS_CAN_STCallBack(&CAN_Frame_Re);
            }
        }
        else if(messageBox==canMESSAGE_BOX16) //小相机
        {
            LOS_CAN_Receive(LOS_CAN_NUM1,&CAN_Frame_Re, messageBox);
            id  = CAN_Frame_Re.ID;
            memcpy(id_filter,&id,4);
            if (id_filter[2]==nID_CMOS)
            {
                LOS_CAN_CMOSCallBack(&CAN_Frame_Re);
            }
        }
        else if(messageBox==canMESSAGE_BOX17) //动力学假磁强计
        {
            LOS_CAN_Receive(LOS_CAN_NUM1,&CAN_Frame_Re, messageBox);
            id  = CAN_Frame_Re.ID;
            memcpy(id_filter,&id,4);
            if (id_filter[2]==nID_MagMe_Dyn)
            {
                LOS_CAN_MagMeCallBack_Dyn(&CAN_Frame_Re);
            }
        }
        else if(messageBox==canMESSAGE_BOX18) //GNSS广播帧
        {
            LOS_CAN_Receive(LOS_CAN_NUM1,&CAN_Frame_Re, messageBox);
            id  = CAN_Frame_Re.ID;
            memcpy(id_filter,&id,4);
            if (id_filter[2]==nID_GNSS)
            {
                LOS_CAN_GNSSCallBack(&CAN_Frame_Re);
            }
        }
        else if(messageBox==canMESSAGE_BOX19)
        {
            LOS_CAN_Receive(LOS_CAN_NUM1,&CAN_Frame_Re, messageBox);
            id  = CAN_Frame_Re.ID;
            if ((id & 0xFF000000) == 0x01000000) //设备识别申请帧
            {
                LOS_CAN_nID(&CAN_Frame_Re,&CAN_Frame_Permit);//nID分配
                LOS_CAN_Transmit(LOS_CAN_NUM1,CAN_Frame_Permit.ID,CAN_Frame_Permit.msg,4);//发送设备识别允许帧
            }

        }

    }
    if(node==canREG2)
    {
        if(messageBox==canMESSAGE_BOX2) //存储板
        {
            LOS_CAN_Receive(LOS_CAN_NUM2,&CAN_Frame_Re, messageBox);
            id  = CAN_Frame_Re.ID;
            memcpy(id_filter,&id,4);
            if (id_filter[2]==nID_Storage)
            {
                LOS_CAN_SATACallBack(&CAN_Frame_Re);
            }
        }
        else if (messageBox==canMESSAGE_BOX3) //星敏Y
        {
            LOS_CAN_Receive(LOS_CAN_NUM2,&CAN_Frame_Re, messageBox);
            id  = CAN_Frame_Re.ID;
            memcpy(id_filter,&id,4);
            if (id_filter[2]==nID_StarSensorY)
            {
                LOS_CAN_SSYCallBack(&CAN_Frame_Re);
            }
        }
        else if (messageBox==canMESSAGE_BOX4) //星敏Z
        {
            LOS_CAN_Receive(LOS_CAN_NUM2,&CAN_Frame_Re, messageBox);
            id  = CAN_Frame_Re.ID;
            memcpy(id_filter,&id,4);
            if (id_filter[2]==nID_StarSensorZ)
            {
                LOS_CAN_SSZCallBack(&CAN_Frame_Re);
            }
        }
        else if (messageBox==canMESSAGE_BOX5) //其他帧
        {
            LOS_CAN_Receive(LOS_CAN_NUM2,&CAN_Frame_Re, messageBox);
            id  = CAN_Frame_Re.ID;
            if ((id & 0xFF000000) == 0x01000000) //设备识别申请帧
            {
                LOS_CAN_nID(&CAN_Frame_Re,&CAN_Frame_Permit);//nID分配
                LOS_CAN_Transmit(LOS_CAN_NUM2,CAN_Frame_Permit.ID,CAN_Frame_Permit.msg,4);//发送设备识别允许帧
            }
            else if ((id & 0xFF000000) == 0x06000000) //地面调试专用指令
            {
                LOS_CAN_Debug(&CAN_Frame_Re);
            }
        }
    }
}


/** @fn uint8_t LOS_CAN_nID(LOS_CANMSG_t* CAN_Frame_Apply)  
*   @brief CAN总线nID分配函数
*   @param[in] CAN_Frame_Apply 设备识别申请帧内容（ID与DATA）
*   @param[in] CAN_Frame_Permit 设备识别允许帧内容（ID与DATA）
*/
uint8_t LOS_CAN_nID(LOS_CANMSG_t* CAN_Frame_Apply, LOS_CANMSG_t* CAN_Frame_Permit)
{
    uint32_t partoID = CAN_Frame_Apply->ID;
    uint8_t nID=0x00;
    //0x0153510C
    switch (partoID)
    {
        //存储板
        case LOS_oID1_Storage:
        CAN_Frame_Permit->ID = LOS_oID2_Storage;
        nID = nID_Storage;
        STA_SATA.REC = 1;
        break;
        
        //测控板
        case LOS_oID1_TTCBoard:
        CAN_Frame_Permit->ID = LOS_oID2_TTCBoard;
        tempf = 1;
        nID = nID_TTCBoard;
        break;
        
        //数传基带
        case LOS_oID1_Baseband:
        CAN_Frame_Permit->ID = LOS_oID2_Baseband;
        tempf = 1;
        nID = nID_BaseBand;
        STA_DT.REC = 1;
        STA_DT.askcnt = 0;
        break;
        
        //星敏Y
        case LOS_oID1_StarSensorY1:
        CAN_Frame_Permit->ID = LOS_oID2_StarSensorY1;
        nID = nID_StarSensorY;
        STA_SSY.FSM = 0;
        STA_SSY.REC = 1;  //已识别到设备
        break;
        
        //星敏Z
        case LOS_oID1_StarSensorZ1:
        CAN_Frame_Permit->ID = LOS_oID2_StarSensorZ1;
        nID = nID_StarSensorZ;
        STA_SSZ.FSM = 0;
        STA_SSZ.REC = 1;  //已识别到设备
        break;

        //星敏Y*
        case LOS_oID1_StarSensorY2:
        CAN_Frame_Permit->ID = LOS_oID2_StarSensorY2;
        nID = nID_StarSensorY;
        STA_SSY.FSM = 0;
        STA_SSY.REC = 1;  //已识别到设备
        break;
        
        //星敏Z*
        case LOS_oID1_StarSensorZ2:
        CAN_Frame_Permit->ID = LOS_oID2_StarSensorZ2;
        nID = nID_StarSensorZ;
        STA_SSZ.FSM = 0;
        STA_SSZ.REC = 1;  //已识别到设备
        break;
        
        //太敏
        case LOS_oID1_SunSensor:
        CAN_Frame_Permit->ID = LOS_oID2_SunSensor;
        nID = nID_SunSensor;
        STA_SUNS.REC = 1;
        break;
        
        //飞轮X
        case LOS_oID1_WheelX1:
        CAN_Frame_Permit->ID = LOS_oID2_WheelX1;
        nID = nID_WheelX;
        STA_FW[0].REC = 1;
        break;

        //飞轮X*
        case LOS_oID1_WheelX2:
        CAN_Frame_Permit->ID = LOS_oID2_WheelX2;
        nID = nID_WheelX;
        STA_FW[0].REC = 1;
        break;
        
        //飞轮Y
        case LOS_oID1_WheelY1:
        CAN_Frame_Permit->ID = LOS_oID2_WheelY1;
        nID = nID_WheelY;
        STA_FW[1].REC = 1;
        break;

        //飞轮Y*
        case LOS_oID1_WheelY2:
        CAN_Frame_Permit->ID = LOS_oID2_WheelY2;
        nID = nID_WheelY;
        STA_FW[1].REC = 1;
        break;
        
        //飞轮Z
        case LOS_oID1_WheelZ1:
        CAN_Frame_Permit->ID = LOS_oID2_WheelZ1;
        nID = nID_WheelZ;
        STA_FW[2].REC = 1;
        break;

        //飞轮Z*
        case LOS_oID1_WheelZ2:
        CAN_Frame_Permit->ID = LOS_oID2_WheelZ2;
        nID = nID_WheelZ;
        STA_FW[2].REC = 1;
        break;

        //飞轮S
        case LOS_oID1_WheelS1:
        CAN_Frame_Permit->ID = LOS_oID2_WheelS1;
        nID = nID_WheelS;
        STA_FW[3].REC = 1;
        break;

        //飞轮S*
        case LOS_oID1_WheelS2:
        CAN_Frame_Permit->ID = LOS_oID2_WheelS2;
        nID = nID_WheelS;
        STA_FW[3].REC = 1;
        break;

        //GNSS板
        case LOS_oID1_GNSS:
        CAN_Frame_Permit->ID = LOS_oID2_GNSS;
        nID = nID_GNSS;
        STA_GNSS.REC = 1;
        break;

        //增阻球
        case LOS_oID1_Ball:
        CAN_Frame_Permit->ID = LOS_oID2_Ball;
        nID = nID_Ball;
        break;

        //导航增强
        case LOS_oID1_NaviEnhance:
        CAN_Frame_Permit->ID = LOS_oID2_NaviEnhance;
        nID = nID_NaviEnhance;
        //connect = 1;
        STA_NaviEnhance.CMD = 1;
        break;

        //时空表
        case LOS_oID1_Spacetime:
        CAN_Frame_Permit->ID = LOS_oID2_Spacetime;
        nID = nID_Spacetime;
        break;

        //冷气推进
        case LOS_oID1_GP:
        CAN_Frame_Permit->ID = LOS_oID2_GP;
        nID = nID_GP;
        break;

        //AIS接收机
        case LOS_oID1_AIS:
        CAN_Frame_Permit->ID = LOS_oID2_AIS;
        nID = nID_AIS;
        break;

        //原子氧探测器
        case LOS_oID1_AtomOxygen:
        CAN_Frame_Permit->ID = LOS_oID2_AtomOxygen;
        nID = nID_AtomOxygen;
        break;

        //搭载计算机
        case LOS_oID1_BMOBC:
        CAN_Frame_Permit->ID = LOS_oID2_BMOBC;
        nID = nID_BMOBC;
        break;

        //CMOS相机
        case LOS_oID1_CMOS:
        CAN_Frame_Permit->ID = LOS_oID2_CMOS;
        nID = nID_CMOS;
        break;

        default:
        nID = 0X00;
    }

    CAN_Frame_Permit->msg[0]=CAN_Frame_Apply->msg[0];
    CAN_Frame_Permit->msg[1]=CAN_Frame_Apply->msg[1];
    CAN_Frame_Permit->msg[2]=CAN_Frame_Apply->msg[2];
    CAN_Frame_Permit->msg[3]=nID;
}

/** @fn void LOS_CAN_FirstPart(LOS_SNP_t* SNP, uint8_t* FirstPart)  
*   @brief CAN总线数据传输首部生成器
*   @param[in] SNP 首部信息结构体指针
*   @param[in] FirstPart 生成的首部地址
*/
void LOS_CAN_FirstPart(LOS_SNP_t* SNP, uint8_t* FirstPart)
{
    uint16_t length = 0;
    uint8_t sum = 0;
    length = SNP->SNP_len;
    length = length << 2;
    memcpy(&FirstPart[0], &length, 2);
    FirstPart[0] = FirstPart[0] | 0x20;
    FirstPart[1] = FirstPart[1] | 0x03;
    FirstPart[2] = ((SNP->SNP_tran) << 4) | 0X0F;
    FirstPart[3] = SNP->SNP_source;
    FirstPart[4] = SNP->SNP_dest;
    FirstPart[5] = 0XFF;
    FirstPart[6] = 0xF0 | (SNP->SNP_rec);
    sum = FirstPart[0] + FirstPart[1] + FirstPart[2] + FirstPart[3] + FirstPart[4] + FirstPart[5] + FirstPart[6];
    FirstPart[7] = 0x100 - sum;
}


/** @fn uint32_t LOS_CAN_IDGen(uint8_t CAN_Frame_Type, uint8_t srcnID, uint8_t desnID, uint8_t order_num)  
*   @brief CAN数据传输帧仲裁域生成器
*   @param[in] ACK_Type 应答类型 ACK_Need  ACK_NoNeed  ACK_Self
*   @param[in] srcnID 发送方nID
*   @param[in] desnID 接收方nID
*   @param[in] order_num 帧序号
*/
uint32_t LOS_CAN_IDGen(uint8_t ACK_Type, uint8_t srcnID, uint8_t desnID, uint8_t order_num)
{
    uint32_t CANID=0;
    CANID = CANID | (0b110<<26);

    if (ACK_Type == ACK_Need)
    {
        CANID = CANID | (0b00 << 24);
    }
    else if (ACK_Type == ACK_NoNeed)
    {
        CANID = CANID | (0b01 << 24);
    }
    else if (ACK_Type == ACK_Self)
    {
        CANID = CANID | (0b10 << 24);
    }

    CANID = CANID | (desnID << 16);
    CANID = CANID | (srcnID << 8);
    CANID = CANID | (order_num << 0);

    return CANID;
}



void LOS_CAN_Debug(LOS_CANMSG_t* canmsg)
{
    // uint8_t sd_check;
    // uint16_t sd_cnt = 0;
    // uint16_t i = 0;
    // uint32_t capacity = 0;
    // uint32_t id = canmsg -> ID;
    // uint8_t maxr = 0;
    // uint32_t duty = 0;
    
    // uint32_t radd_flash = 0;
    // uint32_t flash_value=0;
    // uint32_t test32 = 0x12345678;
    // uint8_t test84[4]={0,0,0,0};
    // uint32_t tablesize = 0;
    // BaseType_t xHigherPriorityTaskWoken;

    // uint8_t amur1[12] = {0x07,0x10,0x08,0x06,0x07,0xd2,0xff,0x01,0x01,0x01,0x01,0x00};
    // uint8_t rev;


    // #ifdef NOW_FLASH0
    // uint8_t idcode[4]={0x00,0x00,0x00,0x00};
    // #endif
    // #ifdef NOW_FLASH1
    // uint8_t idcode[4]={0x11,0x11,0x11,0x11};
    // #endif
    
    // switch(id)
    // {
    //     case 0x06000000:
    //     LOS_CAN_Radio();
    //     break;
        
    //     case 0x06000001:
    //     LOS_GNSS_CMD(GNSS_CMD_Reset);
    //     break;
        
    //     case 0x06000002:
    //     LOS_GNSS_CMD(GNSS_CMD_TimePolling);
    //     break;

    //     case 0x06000003:
    //     LOS_GNSS_CMD(GNSS_CMD_StatusPolling);
    //     break;

    //     case 0x06000004:
    //     LOS_GNSS_CMD(GNSS_CMD_OriginPolling);
    //     break;

    //     case 0x06000005:
    //     LOS_NaviEnhance_CMD(NaviEnhance_CMD_Status);
    //     break;

    //     case 0x06000006:
    //     //LOS_NaviEnhance_CMD(NaviEnhance_CMD_Result);
    //     break;

    //     case 0x06000007:
    //     LOS_NaviEnhance_OriginData(BUF_GNSS_OriginData);
    //     break;

    //     case 0x06000008:
    //     navi_test_flag = 1;
    //     break;

    //     case 0x06000009:
    //     // for (sd_cnt = 0;sd_cnt < 2048; sd_cnt++)
    //     // {
    //     //     sd_packs_t[sd_cnt] = i;
    //     //     i++;
    //     //     if (i > 255)
    //     //     {
    //     //         i = 0;
    //     //     }
    //     // }
    //     // sd_check = LOS_SD_WriteMultiBlock(200, sd_packs_t, 4);
    //     //     LOS_CAN_DoNothing();
    //     break;

    //     case 0x0600000A:
    //     //sd_check = LOS_SD_ReadMultiBlock(200, sd_packs_r, 4);
    //     break;

    //     case 0x0600000B:
    //     //sd_check = LOS_SD_ReadMultiBlock(198, sd_packs_r, 4);
    //     break;

    //     case 0x0600000C:
    //    // sd_check = LOS_SD_ReadMultiBlock(202, sd_packs_r, 4);
    //     break;

    //     case 0x0600000D:
    //     sd_flag = 1;
    //     //sd_check = LOS_FATFS_TestWrite();
    //     fatfs_arg = canmsg->msg[0];
    //     break;

    //     case 0x0600000E:
    //     sd_flag = 2;
    //     fatfs_arg = canmsg->msg[0];
    //     break;

    //     case 0x0600000F:
    //     capacity = LOS_SD_GetCapacity();
    //     LOS_CAN_Transmit(LOS_CAN_NUM1,0x06000000,&capacity,4);
    //     break;

    //     case 0x06000010:
    //     sd_flag = 3;
    //     break;

    //     case 0x06000011:
    //     sd_flag = 4;
    //     break;

    //     case 0x06000012:
    //     sd_flag = 5;
    //     break;

    //     case 0x06000013:
    //     sd_flag = 6;
    //     break;

    //     case 0x06000014:
    //     sd_flag = 7;
    //     break;

    //     case 0x06000015:
    //     max_flag = 1;
    //     break;

    //     case 0x06000016:
    //     max_flag = 2;
    //     break;

    //     case 0x06000017:
    //     max_flag = 3;
    //     break;

    //     case 0x06000018:
    //     max_flag = 4;
    //     break;

    //     case 0x06000019:
    //     //maxr= LOS_MAX14830_ReadRegister(LOS_MAX14830_NUM1, 0, canmsg->msg[0]);
    //     //LOS_CAN_Transmit(LOS_CAN_NUM1, 0x06191919, &maxr, 1);
    //     LOS_MAX14830_Check();
    //     //LOS_YH50_SendCMD(0x77);
    //     break;

    //     case 0x0600001A:
    //     LOS_MagMe_PWROn();
    //     break;

    //     case 0x0600001B:
    //     LOS_MagMe_PWROff();
    //     break;

    //     case 0x0600001C:
    //     LOS_CAN_Transmit(LOS_CAN_NUM1, 0x06111111, idcode, 4);
    //     break;

    //     case 0x0600001D:
    //     memcpy(&radd_flash, canmsg->msg, 4);
    //     memcpy(&flash_value, radd_flash, 4);
    //     LOS_CAN_Transmit(LOS_CAN_NUM1, 0x06000000, &flash_value, 4);
    //     break;

    //     case 0x06000020:
    //     LOS_StarSensor_CMD(StarSensorY,  StarCMD_Check);
    //     break;

    //     case 0x06000021:
    //     LOS_CAN_Transmit(LOS_CAN_NUM1, 0x06191919, &LOS_Time_Sec, 4);
    //     break;

    //     case 0x06000022:
    //     LOS_CAN_Transmit(LOS_CAN_NUM1, 0x06191919, &LOS_Time_MilSec, 4);
    //     break;

    //     case 0x06000023:
    //     if (test_bit == 1)
    //     {
    //         test_bit = 0;
    //     }
    //     else
    //     {
    //         test_bit = 1;
    //     }
    //     break;

    //     case 0x06000024:
    //     signal_test.period = 250000;
    //     signal_test.duty = canmsg->msg[0];
    //     break;

    //     case 0x06000025:
    //     tablesize = LOS_FP_GetTableSize(phead);
    //     LOS_CAN_Transmit(LOS_CAN_NUM1, 0x06f0f0f1, &tablesize, 4);
    //     break;

    //     case 0x06000026:
    //     {
    //         uint32_t tasknum = 0;
    //         FLIGHTTASK_t* BUF;
    //         memcpy(&tasknum, &(canmsg->msg[0]), 4);
    //         BUF = LOS_FP_SearchTask(tasknum);
    //         LOS_CAN_Transmit(LOS_CAN_NUM1, 0x06f0f0f1, &(BUF->arg[0]), 8);
    //         LOS_CAN_Transmit(LOS_CAN_NUM1, 0x06f0f0f2, &(BUF->arg[8]), 8);
    //     }
    //     break;

    //     case 0x06000027:
    //     flash_sta = LOS_Flash_EraseBanks(Fapi_FlashBank1);
    //     LOS_CAN_Transmit(LOS_CAN_NUM1, 0x06f0f0f1, &flash_sta, 1);
    //     break;

    //     case 0x06000028:
    //     //flash_sta = LOS_Flash_WriteData(Fapi_FlashBank7, 1, 0, eeprom_write, 32);
    //     LOS_CAN_Transmit(LOS_CAN_NUM1, 0x06f0f0f0, &flash_sta, 1);
    //     break;

    //     case 0x06000029:
    //     //flash_sta = LOS_Flash_WriteData(Fapi_FlashBank1, 0, 0, readbuf, 128);
    //     //LOS_CAN_Transmit(LOS_CAN_NUM1, 0x06f0f0f0, &flash_sta, 1);
    //     RTOS_FLAG_Update1 = 1;
    //     break;

    //     case 0x0600002A:
    //     //flash_sta = LOS_Flash_WriteData(Fapi_FlashBank1, 14, 0, flash_write1, 128);
    //     LOS_CAN_Transmit(LOS_CAN_NUM1, 0x06f0f0f0, &flash_sta, 1);
    //     break;

    //     case 0x0600002B:
    //     flash_sta = LOS_Flash_EraseSector(Fapi_FlashBank1,1);
    //     LOS_CAN_Transmit(LOS_CAN_NUM1, 0x06f0f0f1, &flash_sta, 1);
    //     break;

    //     case 0x0600002C:
    //     flash_sta = LOS_Flash_EraseSector(Fapi_FlashBank7,1);
    //     LOS_CAN_Transmit(LOS_CAN_NUM1, 0x06f0f0f1, &flash_sta, 1);
    //     break;

    //     case 0x0600002D:
    //     PERMIT_UPLOAD_JUMP=10;  //复位后从FLASH0运行
    //     break;

    //     case 0x0600002E:
    //     PERMIT_UPLOAD_JUMP=20;  //复位后从FLASH1运行
    //     break;

    //     case 0x0600002F:

    //     break;

    //     case 0x06000030:
    //     LOS_SATA_GetState();
    //     break;

    //     case 0x06000031:
        
    //     //LOS_SATA_Control(0x00000020,0x00000030,OP1_SartCamToSATA,OP2_NOP);
    //     break;

    //     case 0x06000032:
    //     //LOS_SATA_Control(0x00000000,0x00000000,OP1_NOP,OP2_ResetSATA);
    //     break;

    //     case 0x06000033:
    //     LOS_FW_GetState(FW_Z);
    //     break;

    //     case 0x06000034:
    //     {
    //         float angvel = 0;
    //         memcpy(&angvel,&(canmsg->msg[0]),4);
    //         //LOS_FW_SetAngvel(FW_X,angvel);
    //         break;
    //     }

    //     case 0x06000035:
    //     {
    //         float torque = 0;
    //         memcpy(&torque,&(canmsg->msg[0]),4);
    //         //LOS_FW_SetTorque(FW_X,torque);
    //         break;
    //     }

    //     case 0x06000036:
    //     {
    //         float current = 0;
    //         memcpy(&current,&(canmsg->msg[0]),4);
    //         //LOS_FW_SetCurrent(FW_X,current);
    //         break;
    //     }

    //     case 0x06000037:
    //     {
    //         uint8_t state[8];
    //         state[0]=STA_FW[FW_Y].CMD;
    //         state[1]=STA_FW[FW_Y].MSG;
    //         state[2]=STA_FW[FW_Y].VAL;
    //         state[3]=STA_FW[FW_Y].TM_MOD;
    //         state[4]=STA_FW[FW_Y].TM_CRT;
    //         state[5]=STA_FW[FW_Y].TM_RPM;
    //         state[6]=STA_FW[FW_Y].HL_TXF;
    //         state[7]=STA_FW[FW_Y].HL_WCNT;
    //         LOS_CAN_Transmit(LOS_CAN_NUM1,0X07000037,state,8);
    //         break;
    //     }

    //     case 0x06000038:
    //     LOS_FW_PWROn(FW_Y);
    //     LOS_YH50_PWROn();
    //     break;

    //     case 0x06000039:
    //     LOS_FW_PWROff(FW_Y);
    //     LOS_YH50_PWROff();
    //     break;

    //     case 0x0600003A:
    //     LOS_StarSensor_CMD(StarSensorY,StarCMD_Short);
    //     break;

    //     case 0x0600003B:
    //     LOS_CMOS_Snap();
    //     break;

    //     case 0x0600003C:
    //     LOS_CMOS_SendRecentPhoto();
    //     break;

    //     case 0x0600003D:
    //     LOS_CMOS_SendRecentPhotoMsg();
    //     break;

    //     case 0x0600003E:
    //     LOS_CMOS_STA();
    //     break;

    //     case 0x0600003F:
    //     LOS_CMOS_JPEG();
    //     break;       

    //     case 0x06000040:
    //     LOS_YH50_SendCMD(0X0000);
    //     break;

    //     case 0x06000041:
    //     memcpy(test84,&test32,4);
    //     LOS_CAN_Transmit(LOS_CAN_NUM1,0X07000037,test84,1);
    //     break;

    //     case 0x06000042:
    //     LOS_EPS_GetEXSTA();
    //     break;

    //     case 0x06000043:
    //     LOS_SSY_PWROn();
    //     break;

    //     case 0x06000044:
    //     LOS_SSY_PWROff();
    //     break;

    //     case 0x06000045:
    //     LOS_SUNS_PWROn();
    //     break;

    //     case 0x06000046:
    //     LOS_SUNS_PWROff();
    //     break;

    //     case 0x06000047:
    //     LOS_MagMe_PWROn();
    //     break;

    //     case 0x06000048:
    //     LOS_MagMe_PWROff();
    //     break;

    //     case 0x06000049:
    //     LOS_BinSemphr_GiveFromISR(BinSemphr_TC, &xHigherPriorityTaskWoken);
    //     break;

    //     case 0x0600004A:
    //     LOS_FP_DBGPRT();
    //     break;

    //     case 0x0600004B:


    //     break;

    //     case 0x0600004C:
    //     LOS_EPS_Switch(canmsg->msg[0],SWITCH_ON);
    //     break;

    //     case 0x0600004D:
    //     LOS_EPS_Switch(canmsg->msg[0],SWITCH_OFF);
    //     break;

    //     case 0x0600004E:

    //     break;

    //     case 0x0600004F:

    //     break;

    //     case 0x06000050:

    //     break;

    //     case 0x06000051:
    //     LOS_GNSS_CMD(GNSS_CMD_TimePolling);
    //     break;

    //     case 0x06000052:
    //     LOS_NaviEnhance_DataTest();
    //     break;

    //     case 0x06000053:
    //     LOS_NaviEnhance_UP();
    //     break;

    //     case 0x06000054:
    //     STA_NaviEnhance.TM_Type = canmsg->msg[0];
    //     break;

    //     case 0x06000055:
    //     LOS_NaviEnhance_PWROn();
    //     break;

    //     case 0x06000056:
    //     LOS_NaviEnhance_PWROff();
    //     break;

    //     case 0x06000057:
    //     LOS_EPS_ResetPDH();
    //     break;

    //     case 0x06000058:
    //     LOS_MAX14830_Shutdown();
    //     break;

    //     case 0x06000059:
    //     DBG_PORT = canmsg->msg[0];
    //     break;

    //     case 0x0600005A:
    //     LOS_CMOS_FormatFlash();
    //     break;

    //     case 0x0600005B:
    //     LOS_CMOS_DefaultArg();
    //     break;

    //     case 0x0600005C:
    //     LOS_CMOS_AllPhotoMsg();
    //     break;

    //     case 0x0600005D:
    //     LOS_EPS_TCSwitch(canmsg->msg[0], SWITCH_ON);
    //     break;

    //     case 0x0600005E:
    //     LOS_EPS_TCSwitch(canmsg->msg[0], SWITCH_OFF);
    //     break;

    //     case 0x0600005F:
    //     //LOS_EPS_TCSetTemp(canmsg->msg[0],canmsg->msg[1]);
    //     break;

    //     case 0X06000060:
    //     LOS_CMOS_CheckArg();
    //     break;

    //     case 0x06000061:
    //     LOS_CMOS_SetArg(canmsg->msg[0],canmsg->msg[1],canmsg->msg[2],canmsg->msg[3],canmsg->msg[4],canmsg->msg[5]);
    //     break;

    //     case 0x06000062:
    //     rev = LOS_MAX14830_ReadRegister(MAGSEN_MAXNUM, 2, MAX14830_RxFIFOLvl_REG);
    //     LOS_CAN_Transmit(LOS_CAN_NUM1,0X06111162,&rev,1);
    //     break;

    //     case 0x06000063:
    //     LOS_FotonAmur_check();
    //     break;

    //     case 0x06000064:
    //     LOS_EPS_TCSwitch(canmsg->msg[0],SWITCH_ON);
    //     break;

    //     case 0x06000065:
    //     LOS_EPS_TCSwitch(canmsg->msg[0],SWITCH_OFF);
    //     break;

    //     case 0x06000066:
    //     LOS_CIOMPCam_Snap();
    //     break;

    //     case 0x06000067:
    //     LOS_CIOMPCam_Stop();
    //     break;

    //     case 0x06000068:
    //     LOS_CIOMPCam_SetMode(0x55, 0x66, 0xAA, 0x55);
    //     break;

    //     case 0x06000069:
    //     LOS_SATA_Control(0, 0, 0x11, 0x00, 0x00, 0x00, 0x00);
    //     break;

    //     case 0x0600006A:
    //     LOS_SATA_Control(0, 0, 0x44, 0x00, 0x00, 0x00, 0x00); 
    //     break;

    //     case 0x0600006B:
    //     LOS_CAM_PWROn();
    //     break;

    //     case 0x0600006C:
    //     LOS_SATA_Control(0, 0, 0X00, 0X00, 0X00, 0X55, 0X00);
    //     break;

    //     case 0x0600006D:
    //     LOS_SATA_Control(0, 0, 0X00, 0X00, 0X00, 0XAA, 0X00);
    //     break;


    //     default:
    //     break;
    // }
}

void LOS_CAN_Radio()
{
    uint8_t radio[3]={0xFF,0xFF,0xFF};
    LOS_CAN_Transmit(LOS_CAN_NUM1,0x3FFFFFF,radio,3);
    LOS_CAN_Transmit(LOS_CAN_NUM2,0x3FFFFFF,radio,3);
}

void LOS_CAN_DoNothing()
{
    /* 什么都不做 */
}


void LOS_CAN_SendPack(uint8_t CANNUM, uint8_t desnID, uint8_t* pack, uint16_t len)
{
    uint8_t cnt = 0;
    LOS_SNP_t SNP_CMD;    //首部配置结构体
    uint8_t FP_CMD[8];    //首部内容，首部用完以后给其他帧做缓冲区使用
    uint16_t delay = 0;

    SNP_CMD.SNP_dest = desnID;          //目的nID
    SNP_CMD.SNP_source = 0xF0;          //OBC发送
    SNP_CMD.SNP_rec = 0x01;             //内容识别码：
    SNP_CMD.SNP_len = len+8;            //长度（包含首部）
    SNP_CMD.SNP_tran = 0x01;            //信息传递服务
    LOS_CAN_FirstPart(&SNP_CMD,FP_CMD);
    for (delay = 0;delay<1000;delay++);
    LOS_CAN_Transmit(CANNUM, LOS_CAN_IDGen(ACK_NoNeed, 0XF0, SNP_CMD.SNP_dest, 0X00), FP_CMD, 8);//发送首部
    while(len != 0)
    {
        if (len > 8)
        {
            for (delay = 0;delay<1000;delay++);
            LOS_CAN_Transmit(CANNUM, LOS_CAN_IDGen(ACK_NoNeed,0xF0,SNP_CMD.SNP_dest,cnt+1), &pack[cnt*8], 8);
            cnt++;
            len -= 8;
        }
        else
        {
            for (delay = 0;delay<1000;delay++);
            LOS_CAN_Transmit(CANNUM, LOS_CAN_IDGen(ACK_NoNeed,0xF0,SNP_CMD.SNP_dest,0xFF), &pack[cnt*8], len);
            len = 0;
        }
    }
}



void LOS_CAN_SendPackDyn(uint8_t* pack, uint8_t len)
{
    uint8_t last = 0;
    uint8_t cnt = 0;
    LOS_SNP_t SNP_CMD;    //首部配置结构体
    uint8_t FP_CMD[8];    //首部内容，首部用完以后给其他帧做缓冲区使用

    SNP_CMD.SNP_dest = nID_BaseBand;            //地测专用nID
    SNP_CMD.SNP_source = 0xF0;          //OBC发送
    SNP_CMD.SNP_rec = 0x00;             //内容识别码：宽带上传模块接收
    SNP_CMD.SNP_len = len+8;            //长度（包含首部）
    SNP_CMD.SNP_tran = 0x01;            //信息传递服务
    LOS_CAN_FirstPart(&SNP_CMD,FP_CMD);
    LOS_CAN_Transmit(LOS_CAN_NUM2, LOS_CAN_IDGen(ACK_NoNeed, 0XF0, SNP_CMD.SNP_dest, 0X00), FP_CMD, 8);//发送首部

    while(len != 0)
    {
        if (len >= 8)
        {
            LOS_CAN_Transmit(LOS_CAN_NUM2, LOS_CAN_IDGen(ACK_NoNeed,0xF0,SNP_CMD.SNP_dest,cnt+1), &pack[cnt*8], 8);
            cnt++;
            len -= 8;
        }
        else
        {
            LOS_CAN_Transmit(LOS_CAN_NUM2, LOS_CAN_IDGen(ACK_NoNeed,0xF0,SNP_CMD.SNP_dest,0xFF), &pack[cnt*8], len);
            len = 0;
        }
    }
}




//NaviEnhance_CMD_Result
